{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "f8bfc15c",
   "metadata": {},
   "source": [
    "# PVTOL Linear Quadratic Regulator Example\n",
    "\n",
    "Richard M. Murray, 25 Jan 2022\n",
    "\n",
    "This notebook contains an example of LQR control applied to the PVTOL system.  It demonstrates how to construct an LQR controller and also the importance of the feedforward component of the controller.  A gain scheduled design is also demonstrated."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c120d65c",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import control as ct"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "77e2ed47",
   "metadata": {},
   "source": [
    "## System description\n",
    "\n",
    "We use the PVTOL dynamics from the textbook, which are contained in the `pvtol` module.  The vehicle model is both an I/O system model and a flat system model (for the case when the viscous damping coefficient $c$ is zero).\n",
    "\n",
    "<table>\n",
    "<tr>\n",
    "    <td width=\"50%\"><img src=\"https://fbswiki.org/wiki/images/7/76/Pvtol.png\" width=240></td>\n",
    "    <td width=\"50%\">\n",
    "$$\n",
    "\\begin{aligned}\n",
    "  m \\ddot x &= F_1 \\cos\\theta - F_2 \\sin\\theta - c \\dot x, \\\\\n",
    "  m \\ddot y &= F_1 \\sin\\theta + F_2 \\cos\\theta - m g - c \\dot y, \\\\\n",
    "  J \\ddot \\theta &= r F_1.\n",
    "\\end{aligned}\n",
    "$$\n",
    "    </td>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0a12fc3d",
   "metadata": {},
   "source": [
    "The parameter values for the PVTOL system come from the Caltech ducted fan experiment, shown in the video below (the wing forces are not included in the PVTOL model):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7adc6cf1",
   "metadata": {},
   "outputs": [],
   "source": [
    "from IPython.display import YouTubeVideo\n",
    "display(YouTubeVideo('ZFb5kFpgCm4', width=640, height=480))\n",
    "\n",
    "from pvtol import pvtol, plot_results\n",
    "print(pvtol)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "45259984",
   "metadata": {},
   "source": [
    "Since we will be creating a linear controller, we need a linear system model.  We obtain that model by linearizing the dynamics around an equilibrium point.  This can be done in python-control using the `find_eqpt` function.  We fix the output of the system to be zero and find the state and inputs that hold us there."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ea50d7cd",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Find the equilibrium point corresponding to hover\n",
    "xeq, ueq = ct.find_eqpt(pvtol, np.zeros(6), np.zeros(2), y0=np.zeros(6), iy=[0, 1])\n",
    "\n",
    "print(\"xeq = \", xeq)\n",
    "print(\"ueq = \", ueq)\n",
    "\n",
    "# Get the linearized dynamics\n",
    "linsys = pvtol.linearize(xeq, ueq)\n",
    "print(linsys)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7cb8840b",
   "metadata": {},
   "source": [
    "## Linear quadratic regulator (LQR) design\n",
    "\n",
    "Now that we have a linearized model of the system, we can compute a controller using linear quadratic regulator theory.  We seek to find the control law that minimizes the function\n",
    "\n",
    "$$\n",
    "J(x(\\cdot), u(\\cdot)) = \\int_0^\\infty x^T(\\tau) Q_x x(\\tau) + u^T(\\tau) Q_u u(\\tau)\\, d\\tau\n",
    "$$\n",
    "\n",
    "The weighting matrices $Q_x \\in \\mathbb{R}^{n \\times n}$ and $Q_u \\in \\mathbb{R}^{m \\times m}$ should be chosen based on the desired performance of the system (tradeoffs in state errors and input magnitudes).  See Example 3.5 in OBC for a discussion of how to choose these weights.  For now, we just choose identity weights for all states and inputs."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5cfa1ba7",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Start with a diagonal weighting\n",
    "Qx1 = np.diag([1, 1, 1, 1, 1, 1])\n",
    "Qu1 = np.diag([1, 1])\n",
    "K, X, E = ct.lqr(linsys, Qx1, Qu1)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "863d07de",
   "metadata": {},
   "source": [
    "To create a controller for the system, we need to create an I/O system that takes in the desired trajectory $(x_\\text{d}, u_\\text{d})$ and the current state $x$ and generates the control law\n",
    "\n",
    "$$\n",
    "u = u_\\text{d} - K (x - x_\\text{d})\n",
    "$$\n",
    "\n",
    "The function `create_statefbk_iosystem()` does this (see [documentation](https://python-control.readthedocs.io/en/0.9.3.post2/generated/control.create_statefbk_iosystem.html) for details)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5db704e6",
   "metadata": {},
   "outputs": [],
   "source": [
    "control, pvtol_closed = ct.create_statefbk_iosystem(pvtol, K)\n",
    "print(control, \"\\n\")\n",
    "print(pvtol_closed)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bedcb0c0",
   "metadata": {},
   "source": [
    "## Closed loop system simulation\n",
    "\n",
    "We now generate a trajectory for the system and track that trajectory.\n",
    "\n",
    "For this simple example, we take the system input to be a \"step\" input that moves the system 1 meter to the right.  More complex trajectories (eg, using the results from HW #3) could also be used."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a497aa2c",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Generate a step response by setting xd, ud\n",
    "Tf = 15\n",
    "T = np.linspace(0, Tf, 100)\n",
    "xd = np.outer(np.array([1, 0, 0, 0, 0, 0]), np.ones_like(T))\n",
    "ud = np.outer(ueq, np.ones_like(T))\n",
    "ref = np.vstack([xd, ud])\n",
    "\n",
    "response = ct.input_output_response(pvtol_closed, T, ref, xeq)\n",
    "plot_results(response.time, response.states, response.outputs[6:])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f014e660",
   "metadata": {},
   "source": [
    "The limitations of the linear controlller can be seen if we take a larger step, say 10 meters."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a141f100",
   "metadata": {},
   "outputs": [],
   "source": [
    "xd = np.outer(np.array([10, 0, 0, 0, 0, 0]), np.ones_like(T))\n",
    "ref = np.vstack([xd, ud])\n",
    "response = ct.input_output_response(pvtol_closed, T, ref, xeq)\n",
    "plot_results(response.time, response.states, response.outputs[6:])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8adb6ff4",
   "metadata": {},
   "source": [
    "We see that the large initial error causes the vehicle to rotate to a very high role angle (almost 1 radian $\\approx 60^\\circ$), at which point the linear model is not very accurate and the controller errors in the $y$ direction get very large.\n",
    "\n",
    "One way to fix this problem is to change the gains on the controller so that we penalize the $y$ error more and try to keep that error from building up.  However, given the fact that we are trying to stabilize a point that is fairly far from our initial condition, it can be difficult to manage the tradesoffs to get good performance.\n",
    "\n",
    "An alterntaive approach is is to stabilize the system around a trajectory that moves from the initial to final condition.  As a very simple approach, we start by using a _nonfeasible_ trajectory that goes from 0 to 10 in 10 seconds."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a075a0a7",
   "metadata": {},
   "outputs": [],
   "source": [
    "timepts = np.linspace(0, 15, 100)\n",
    "xf = np.array([10, 0, 0, 0, 0, 0])\n",
    "xd = np.array([xf/10 * t if t < 10 else xf for t in timepts]).T\n",
    "ud = np.outer(ueq, np.ones_like(timepts))\n",
    "ref = np.vstack([xd, ud])\n",
    "response = ct.input_output_response(pvtol_closed, timepts, ref, xeq)\n",
    "plot_results(response.time, response.states, response.outputs[6:])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "73d74c23",
   "metadata": {},
   "source": [
    "Note that even though the trajectory was not feasible (it asked the system to move sideways while remaining pointed in the vertical ($\\theta = 0$) direction, the controller has very good performance."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b7539806",
   "metadata": {},
   "source": [
    "## Gain scheduled controller design"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "23d7e21c",
   "metadata": {},
   "source": [
    "Another challenge in using linearized models is that they are only accurate near the point in which they were computed.  For the PVTOL system, this can be a problem if the roll angle $\\theta$ gets large, since in this case the linearization changes significantly (the forces $F_1$ and $F_2$ are no longer aligned with the horizontal and vertical axes).\n",
    "\n",
    "One approach to solving this problem is to compute different gains at different points in the operating envelope of the system.  The code below illustrates the use of gain scheduling by modifying the system drag to a very high value (so that the vehicle must roll to a large angle in order to move sideways against the high drag) and then demonstrates the difficulty in obtaining good performance while trying to track the (still infeasible) trajectory."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4590b138",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Increase the viscous drag to force larger angles\n",
    "linsys = pvtol.linearize(xeq, ueq, params={'c': 20})\n",
    "\n",
    "# Change to physically motivated gains\n",
    "Qx3 = np.diag([10, 100, (180/np.pi) / 5, 0, 0, 0])\n",
    "Qu3 = np.diag([10, 1])\n",
    "\n",
    "# Compute a single gain around hover\n",
    "K, X, E = ct.lqr(linsys, Qx3, Qu3)\n",
    "control, pvtol_closed = ct.create_statefbk_iosystem(pvtol, K)\n",
    "\n",
    "# Simulate the response trying to track horizontal trajectory\n",
    "response = ct.input_output_response(pvtol_closed, T, ref, xeq, params={'c': 20})\n",
    "plot_results(response.time, response.states, response.outputs[6:])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9e01104a",
   "metadata": {},
   "source": [
    "Note that the angle $\\theta$ is quite large (-0.5 rad) during the initla portion of the trajectory, and at this angle (~30$^\\circ$) it is difficult to maintain our altitude while moving sideways.  This happens in large part becuase the system model that we used was linearized about the $\\theta = 0$ configuration.\n",
    "\n",
    "This problem can be addressed by designing a gain scheduled controller in which we compute different system gains at different roll angles.  We carry out those computations below, using the `create_statefbk_iosystem` function, but now passing a set of gains and points instead of just a single gain.\n",
    "\n",
    "(Note: there is a bug in control-0.9.3 that requires gain scheduling to be done on two or more variables, so we also schedule on the horizontal velocity $\\dot x$, even though that doesn't matter that much here.)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e427459f",
   "metadata": {},
   "outputs": [],
   "source": [
    "import itertools\n",
    "import math\n",
    "\n",
    "# Set up points around which to linearize (control-0.9.3: must be 2D or greater)\n",
    "angles = np.linspace(-math.pi/3, math.pi/3, 10)\n",
    "speeds = np.linspace(-10, 10, 3)\n",
    "points = list(itertools.product(angles, speeds))\n",
    "\n",
    "# Compute the gains at each design point\n",
    "gains = []\n",
    "for point in points:\n",
    "    # Compute the state that we want to linearize about\n",
    "    xgs = xeq.copy()\n",
    "    xgs[2], xgs[3] = point[0], point[1]\n",
    "    \n",
    "    # Linearize the system and compute the LQR gains\n",
    "    linsys = pvtol.linearize(xgs, ueq, params={'c': 20})\n",
    "    K, X, E = ct.lqr(linsys, Qx3, Qu3)\n",
    "    gains.append(K)\n",
    "    \n",
    "# Create a gain scheduled controller off of the current state\n",
    "control, pvtol_closed = ct.create_statefbk_iosystem(\n",
    "    pvtol, (gains, points), gainsched_indices=['x2', 'x3'])\n",
    "\n",
    "# Simulate the response\n",
    "response = ct.input_output_response(pvtol_closed, T, ref, xeq, params={'c': 20})\n",
    "plot_results(response.time, response.states, response.outputs[6:])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7399db70",
   "metadata": {},
   "source": [
    "We see that the response is much better, with about 10X less error in the $y$ coordinate."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c8021347",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
